#include <cvblob.h>

using namespace cvb;

// combine two images into one
cv::Mat combineImages(cv::Mat matchingframe,cv::Mat currentFrame)
{
	int width = int(floor(double(matchingframe.cols)/1));
	int height = int(floor(double(matchingframe.rows)/1));

	cv::resize(matchingframe,matchingframe,cv::Size(width,height));
	cv::resize(currentFrame,currentFrame,cv::Size(width,height));
	cv::Mat frameOut(matchingframe.rows,matchingframe.cols + currentFrame.cols + 1,CV_8UC3);
	cv::Mat imageROI;
	imageROI= frameOut(cv::Rect(0,0,matchingframe.cols,matchingframe.rows));
	matchingframe.copyTo(imageROI);
	
	imageROI=frameOut(cv::Rect(matchingframe.cols + 1,0,currentFrame.cols,currentFrame.rows));
	currentFrame.copyTo(imageROI);
	return frameOut;
}

// combine two images into one
cv::Mat combineImagesinVert(cv::Mat matchingframe,cv::Mat currentFrame)
{
	int width = int(floor(double(matchingframe.cols)/1));
	int height = int(floor(double(matchingframe.rows)/1));

	cv::resize(matchingframe,matchingframe,cv::Size(width,height));
	cv::resize(currentFrame,currentFrame,cv::Size(width,height));
	cv::Mat frameOut(matchingframe.rows + currentFrame.rows + 1,matchingframe.cols,CV_8UC3);
	cv::Mat imageROI;
	imageROI= frameOut(cv::Rect(0,0,matchingframe.cols,matchingframe.rows));
	matchingframe.copyTo(imageROI);
	
	imageROI=frameOut(cv::Rect(0,matchingframe.rows+1,currentFrame.cols,currentFrame.rows));
	currentFrame.copyTo(imageROI);
	return frameOut;
}

#define EXAMPLE2

#ifdef EXAMPLE1
//Example 1
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
 
using namespace cv;
using namespace std;
 
int main()
{
    Mat src = imread("../Images/test_20140710_163539.jpg", 0);

	imshow("source", src);
	waitKey(1000);
 
    // Create a structuring element (SE)
    int morph_size = 2;
    Mat element = getStructuringElement( MORPH_RECT, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );
    cout<<element;
 
    Mat dst; // result matrix
    // Apply the specified morphology operation
    for (int i=1;i<10;i++)
    {   
		morphologyEx( src, dst, MORPH_GRADIENT, element, Point(-1,-1), i );   
		//morphologyEx( src, dst, MORPH_TOPHAT, element ); // here iteration=1
		threshold(dst,dst,30,255,CV_THRESH_BINARY);
    
		imshow("result", dst);

		int stopc = waitKey(1000);

		while (stopc == 112)
		{
			int tmp = stopc;
			
			if (stopc = cvWaitKey(1) != 112)
				stopc = tmp;
		}
    }
	
	IplImage *grey = cvCreateImage(cvSize(dst.cols, dst.rows), IPL_DEPTH_8U, 1);
	grey->imageData = (char *)dst.data;

	IplImage *labelImg = cvCreateImage(cvGetSize(grey),IPL_DEPTH_LABEL,1);

	cvb::CvBlobs blobs;
	unsigned int result = cvLabel(grey, labelImg, blobs);

	std::cout << "Processing frameId = 0" << /*nFrame++ << */std::endl;
	std::cout << " - " << blobs.size() << " blobs and " << result << " pixels: " << std::endl;

	//cvFilterByArea(blobs,5,500);

	std::cout << " - After cvFilterByArea " << blobs.size() << " blobs and " << result << " pixels: " << std::endl;

	IplImage *imgOut = cvCreateImage(cvSize(dst.cols, dst.rows), IPL_DEPTH_8U, 3); cvZero(imgOut);
	cvRenderBlobs(labelImg, blobs, grey, imgOut,CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_BOUNDING_BOX);

	cvShowImage("test", imgOut);

	while (int stopc = cvWaitKey(1) != 27);

    return 0;
}
#endif

#ifdef EXAMPLE2
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>

#include <cv.h>
#include <highgui.h>
 
using namespace cv;
using namespace std;

Mat frame, frame_blur, mask, dst;

int b_size = 1; 
int th_size = 20;
int size = 1;

void ShowCombineImages(const string winname, cv::Mat im11, cv::Mat im12, cv::Mat im21, cv::Mat im22, float resize_cols = 2./3.5, float resize_rows = 2./3.5)
{
	cv::Mat frameOut1 = combineImages(im11, im12);

	cv::Mat frameOut2 = combineImages(im21, im22);

	cv::Mat frameOut = combineImagesinVert(frameOut1, frameOut2);

	cv::Mat frameOutinShow;
	cv::resize(frameOut,frameOutinShow,cv::Size((int)(frameOut.cols*resize_cols),(int)(frameOut.rows*resize_rows)));
			
	cv::imshow(winname, frameOutinShow);
}

void on_size(int dmp)
{
	cout << "Size = " << size << endl;
}

void on_blur(int dmp)
{
	cout << "Blur size = " << b_size << endl;
}

void on_thresh(int dmp)
{
	cout << "Threshold = " << th_size << endl;

	cv::threshold(frame_blur, mask, th_size, 255, CV_THRESH_BINARY);
	mask.copyTo(dst);
}

/// Function header
void on_trackbar_erosion(int dmp)
{
	Mat element = getStructuringElement(cv::MORPH_CROSS, 
		cv::Size(2 * size + 1, 2 * size + 1),
		cv::Point(size, size) );

	cout << "Size = " << size << " Erode\n " << element << endl;
 
	// Apply erosion or dilation on the image
	erode(dst, dst,element);  // dilate(image,dst,element);       
         
	//imshow( "Result window", dst );
}

void on_trackbar_dilation(int dmp)
{
	Mat element = getStructuringElement(cv::MORPH_CROSS, 
		cv::Size(2 * size + 1, 2 * size + 1),
		cv::Point(size, size) );

	cout << "Size = " << size << " Dilate\n " << element << endl;
 
	// Apply erosion or dilation on the image
	dilate(dst, dst,element);       
         
	//imshow( "Result window", dst );
}

void on_morphology(int morph_size = 2)
{
	cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, cv::Size( 2*morph_size + 1, 2*morph_size+1 ), cv::Point( morph_size, morph_size ) );
	cv::morphologyEx(dst, dst, cv::MORPH_GRADIENT, element, cv::Point(-1,-1), 1 );
}

string getTimeinstring()
{
    time_t timeObj;
    time(&timeObj);
    tm *pTime = gmtime(&timeObj);
    char buffer[100];
	sprintf(buffer, "%02d%02d%02d_%02d%02d%02d",    pTime->tm_year+1900,pTime->tm_mon+1,pTime->tm_mday,
		pTime->tm_hour+7, pTime->tm_min, pTime->tm_sec);
    return buffer;
}

int CheckBlobOverlap(CvBlob const *b, CvBlob const *t)
{
	int ax = b->minx - t->maxx;
	int bx = b->maxx - t->minx;
	int ay = b->miny - t->maxy;
	int by = b->maxy - t->miny;

	int ax_bx = ax^bx;

	int ay_by = ay^by;

	int x_y = ax_bx & ay_by;

	if (x_y<0)
		return 1;
	return 0;
}

CvPoint DistantBlob2Blob1(CvBlob const *b, CvBlob const *t)
{
	int dx, dy;

	int ax = b->minx - t->maxx;
	int bx = b->maxx - t->minx;
	int ay = b->miny - t->maxy;
	int by = b->maxy - t->miny;

	int ax_bx = ax^bx;		//Bit XOR
	int ay_by = ay^by;
	int x_y = ax_bx & ay_by;
	
	if ((x_y < 0) | !ax | !bx | !ay | !by)
	{
		dx = 0;
		dy = 0;
	}
	else if (ax_bx < 0)
	{
		dx = 0;
		dy = (ay > 0) ? ay : by;
	}
	else if (ay_by < 0)
	{
		dy = 0;
		dx = (ax > 0) ? ax : bx;
	}
	else
	{
		dx = (ax > 0) ? ax : bx;
		dy = (ay > 0) ? ay : by;
	}

	return cvPoint(dx,dy);
}
double DistantBlob2Blob(CvBlob const *b, CvBlob const *t)
{
	double d1;
	if (b->centroid.x < t->minx)
	{
		if (b->centroid.y < t->miny)
			d1 = MAX(t->minx - b->centroid.x, t->miny - b->centroid.y);
		else if (b->centroid.y>t->maxy)
			d1 = MAX(t->minx - b->centroid.x, b->centroid.y - t->maxy);
		else // if (t->miny < b->centroid.y)&&(b->centroid.y < t->maxy)
			d1 = t->minx - b->centroid.x;
	}
	else if (b->centroid.x > t->maxx)
	{
		if (b->centroid.y<t->miny)
			d1 = MAX(b->centroid.x - t->maxx, t->miny - b->centroid.y);
		else if (b->centroid.y>t->maxy)
			d1 = MAX(b->centroid.x - t->maxx, b->centroid.y - t->maxy);
		else
			d1 = b->centroid.x - t->maxx;
	}
	else // if (t->minx =< b->centroid.x) && (b->centroid.x =< t->maxx)
	{
		if (b->centroid.y<t->miny)
			d1 = t->miny - b->centroid.y;
		else if (b->centroid.y>t->maxy)
			d1 = b->centroid.y - t->maxy;
		else 
			return 0.;
	}

	double d2;
	if (t->centroid.x<b->minx)
	{
		if (t->centroid.y<b->miny)
			d2 = MAX(b->minx - t->centroid.x, b->miny - t->centroid.y);
		else if (t->centroid.y>b->maxy)
			d2 = MAX(b->minx - t->centroid.x, t->centroid.y - b->maxy);
		else // if (b->miny < t->centroid.y)&&(t->centroid.y < b->maxy)
			d2 = b->minx - t->centroid.x;
	}
	else if (t->centroid.x>b->maxx)
	{
		if (t->centroid.y<b->miny)
			d2 = MAX(t->centroid.x - b->maxx, b->miny - t->centroid.y);
		else if (t->centroid.y>b->maxy)
			d2 = MAX(t->centroid.x - b->maxx, t->centroid.y - b->maxy);
		else
			d2 = t->centroid.x - b->maxx;
	}
	else // if (b->minx =< t->centroid.x) && (t->centroid.x =< b->maxx)
	{
		if (t->centroid.y<b->miny)
			d2 = b->miny - t->centroid.y;
		else if (t->centroid.y>b->maxy)
			d2 = t->centroid.y - b->maxy;
		else 
			return 0.;
	}

	return MIN(d1, d2);
}

//void cvFilterByDistant(CvBlobs &blobs, double dis)
CvBlob* Merge2Blob(CvBlob *b, CvBlob *t)
{	
	CvBlob *tmp = b->area >= t->area ? b : t;
	
	if (!CheckBlobOverlap(b, t) || DistantBlob2Blob(b, t))
	{
		tmp->minx = b->minx < t->minx ? b->minx : t->minx;
		tmp->miny = b->miny < t->miny ? b->miny : t->miny;
		tmp->maxx = b->maxx > t->maxx ? b->maxx : t->maxx;
		tmp->maxy = b->maxy > t->maxy ? b->maxy : t->maxy;

		tmp->centroid.x = (tmp->minx + tmp->maxx)/2.0;
		tmp->centroid.y = (tmp->miny + tmp->maxy)/2.0;
		//tmp->centroid=cvPoint2D64f(tmp->m10/tmp->area, tmp->m01/tmp->area);

		tmp->m00 = b->m00 + t->m00;
		tmp->m01 = b->m01 + t->m01;
		tmp->m10 = b->m10 + t->m10;
		tmp->m11 = b->m11 + t->m11;
		tmp->m20 = b->m20 + t->m20;
		tmp->m02 = b->m02 + t->m02;
		
		tmp->area = b->area + t->area;
	}
		
	return tmp;
}

CvBlob* LargestBlob(CvBlobs &blobs)
{
	CvBlob *maxBlob = blobs.begin()->second;
	unsigned int maxArea = maxBlob->area;

	CvBlobs::const_iterator it=blobs.begin();

    for (it++; it!=blobs.end();++it)
    { 
		if (it->second->area > maxArea)
		{
			maxBlob = (*it).second;
			maxArea = it->second->area;
		}
    }

    return maxBlob;
}

void NewMergeBlobs(CvBlobs &blobs)
{
	int m_size = blobs.size();
	int i, j;

	vector<CvPoint> matrix_pos;
	matrix_pos.resize(m_size*(m_size-1)/2);

	vector< vector<CvPoint> > matrix_distance;
	// Set up sizes. (m_size x m_size)
	matrix_distance.resize(m_size);
	for (i = 0; i < m_size; ++i)
		matrix_distance[i].resize(m_size);
	//Calc matrix Distant
	
	int iPos = 0;
	i = 0, j = 0;
	for (CvBlobs::iterator it=blobs.begin(); it != blobs.end(); it++, i++)
	{
		CvBlob *b = (*it).second;
		
		j = i;
		for (CvBlobs::iterator jt = it; jt != blobs.end(); jt++, j++)
		{
			if (jt != it)
			{
				CvBlob *t = (*jt).second;

				CvPoint dis = DistantBlob2Blob1(b, t);

				matrix_distance[i][j] = dis;
				matrix_distance[j][i] = cvPoint(-dis.x, -dis.y);
				cout << "(" << matrix_distance[i][j].x << "," <<matrix_distance[i][j].y << ")\t";
				//Insert Matrix Pos
				if (!iPos)
					matrix_pos[iPos++] = cvPoint(i, j);
				else
				{
					int vt = iPos++;
					
					while ((vt > 0) && (abs(matrix_distance[matrix_pos[vt-1].x][matrix_pos[vt-1].y].x) >= abs(dis.x)))
					{
						if (abs(matrix_distance[matrix_pos[vt-1].x][matrix_pos[vt-1].y].x) > abs(dis.x))
						{
							matrix_pos[vt] = matrix_pos[vt-1];
							vt--;
						}
						else
						{
							while (abs(matrix_distance[matrix_pos[vt-1].x][matrix_pos[vt-1].y].y) > abs(dis.y))
							{
								matrix_pos[vt] = matrix_pos[vt-1];
								vt--;
							}
							break;
						}
					}

					matrix_pos[vt] = cvPoint(i, j);
				}
			}
			else
				matrix_distance[i][j] = cvPoint(999,999);
		}
		std::cout << std::endl;
	}
		
	for (iPos = 0; iPos < matrix_pos.size(); iPos++)
		std::cout << "(" << matrix_pos[iPos].x << "," <<matrix_pos[iPos].y << ")\t";
	std::cout << std::endl << "sdfsdflsdkjfl";

	iPos = 0;

	while (iPos < matrix_pos.size())
	{
		int k = matrix_pos[iPos].x;
		int l = matrix_pos[iPos].y;
		int dxki, dyki, dxli, dyli;

		for (i = 0; i < m_size; i++)
		{
			if (i != k && i != l)
			{
				dxki = matrix_distance[k][i].x;
				dyki = matrix_distance[k][i].y;
				dxli = matrix_distance[l][i].x;
				dyli = matrix_distance[l][i].y;

				if ((dxki^dxli) < 0)
					dxki = dxli = 0;
				else
					dxki = dxli = abs(dxki) < abs(dxli) ? dxki : dxli;

				if ((dyki^dyli) < 0)
					dyki = dyli = 0;
				else
					dyki = dyli = abs(dyki) < abs(dyli) ? dyki : dyli;

				matrix_distance[l][i] = matrix_distance[k][i] = cvPoint(dxki, dyki);
				matrix_distance[i][l] = matrix_distance[i][k] = cvPoint(-dxki, -dyki);
			}
			else
			{
				matrix_distance[l][i] = matrix_distance[k][i] = cvPoint(999,999);
				matrix_distance[i][l] = matrix_distance[i][l] = cvPoint(999,999);
			}

		}

		std::vector<CvPoint>::iterator it = matrix_pos.begin();
		while (it != matrix_pos.end())
		{
			if ((*it).x == l || (*it).y == l)
			{
				
				//vector<CvPoint>::iterator tmp = it;
				//it++;
				//std::cout << "(" << (*tmp).x << "," << (*tmp).y << ")\t";
				matrix_pos.erase(it);
				matrix_pos.shrink_to_fit();
				it = matrix_pos.begin();
			}
			else
				it++;
		}
		iPos++;
	}

	//Find MIN Distant
	int minx = 999;
	int miny = 999;
	int vt = 0;
	for (j = 1; j < m_size; j++)
	{
		if (minx > matrix_distance[0][j].x)
		{
			minx = matrix_distance[0][j].x;
			miny = matrix_distance[0][j].y;
			vt = j;
		}
		else if (minx == matrix_distance[0][j].x)
			if (miny > matrix_distance[0][j].y)
			{
				miny = matrix_distance[0][j].y;
				vt = j;
			}
	}

	for (int ii = 0; ii < m_size; ii++)
	{
		for (int jj = 0; jj < m_size; jj++)
			cout << "(" << matrix_distance[ii][jj].x << "," <<matrix_distance[ii][jj].y << ")\t";
		cout << endl;
	}
}


void MergeBlobsByDistant(CvBlobs &blobs, int dx, int dy)
{		
	CvBlob *firstBlob = new CvBlob;
	//firstBlob = blobs.begin()->second;//*firstBlob = *it->second;
	firstBlob = LargestBlob(blobs);
	
	CvBlobs::iterator it=blobs.begin();
	
    while(it!=blobs.end())
    {
		CvBlob *blob=(*it).second;		
		if (blob != firstBlob)
		{
			CvPoint dis = DistantBlob2Blob1(firstBlob, blob);
			double dist = DistantBlob2Blob(firstBlob, blob);
			//double dis = cvDistancePointPoint(cvPoint(firstBlob->centroid.x, firstBlob->centroid.y),cvPoint( blob->centroid.x, blob->centroid.y));
			cout << "Distant = " << dist << endl;
			cout << "Dx = " << dis.x << ", Dy = " << dis.y << endl;

			//if (dis <= distant)
			if ((abs(dis.x) <= dx) && (abs(dis.y) <= dy))
			{
				firstBlob = Merge2Blob(firstBlob, blob);
			
				cvReleaseBlob(blob);

				CvBlobs::iterator tmp=it++; //tmp = it;
				//++it;
				blobs.erase(tmp);
			}
			else
				++it;
		}
		else
			++it;
	}
}

void ShowBlobsArea(CvBlobs &blobs)
{
	int i = 1;
	for (CvBlobs::iterator it=blobs.begin(); it != blobs.end(); ++it)
	{
		cout << i << ".\t Area = " << it->second->area << endl;
	}
}

void on_blobs(int opt = 1)
{	
	int width = dst.cols;
	int height = dst.rows;

	IplImage *img = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
	IplImage *grey = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
	IplImage *labelImg = cvCreateImage(cvGetSize(img),IPL_DEPTH_LABEL,1);

	img->imageData = (char *)dst.data;
	cvCvtColor(img, grey, CV_BGR2GRAY);

	cvb::CvBlobs blobs;
	unsigned int result = cvLabel(grey, labelImg, blobs);
	if (opt)
		std::cout << " - " << blobs.size() << " blobs and " << result << " pixels: " << std::endl;

	//cvFilterByArea(blobs, 15,0.1*width*height);
	if (blobs.size() > 1)
		MergeBlobsByDistant(blobs, 30, 30);

	//cvFilterByArea(blobs, 15,0.1*width*height);

	if (opt)
		std::cout << " - After cvFilterByArea " << blobs.size() << " blobs and " << result << " pixels: " << std::endl;

	ShowBlobsArea(blobs);

	if (blobs.size() == 4)
		NewMergeBlobs(blobs);

	IplImage *imgOut = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); //cvZero(imgOut);
	imgOut->imageData = (char*)dst.data;
	
	cvRenderBlobs(labelImg, blobs, img, imgOut,CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_BOUNDING_BOX);
	
	dst = cv::Mat(imgOut,true);
		
	cvReleaseImage(&img);
	cvReleaseImage(&grey);
	cvReleaseImage(&labelImg);

	cvReleaseBlobs(blobs);
	cvReleaseImage(&imgOut);
}

int main( )
{
	string fileVideo = "D:\\!Projects\\!ExampleProjects\\BGSubtraction\\Save\\20140712_182105_BGSResult1.avi";
	cv::VideoCapture vcap;

	if (!vcap.open(fileVideo))
	{
		//No load file video
		return -1;
	}

	int width = (int)vcap.get(CV_CAP_PROP_FRAME_WIDTH);
	int height = (int)vcap.get(CV_CAP_PROP_FRAME_HEIGHT);
	
	cv::Mat frame;
	cv::Mat frameOut1, frameOut2, frameOut, frameOutinShow;

	unsigned int nFrame = 0;
	
	if (!vcap.read(frame))
	{
		cout << "No Load frame" << endl;
		return -2;
	}
	std::cout << "Frame Id = " << nFrame++ << endl;

	frame.copyTo(frame_blur);
	frame.copyTo(mask);
	frame.copyTo(dst);
	
	namedWindow( "Result window", CV_WINDOW_AUTOSIZE );
	ShowCombineImages("Result window", frame, frame_blur, mask, dst);
	
	cvCreateTrackbar( "E & D Size: ", "Result window", &size, 20, on_size );
	cvCreateTrackbar( "Blur KSize: ", "Result window", &b_size, 5, on_blur );
	cvCreateTrackbar( "Threshold : ", "Result window", &th_size, 255, on_thresh );
			
	bool isRunStep = false;
	bool isViewZoom = true;
	//int key = 112 ^ 27 ^ 'm' ^ 't' ^ 'b' ^ '-' ^ '+' ^ '[' ^ ']' ^ '\\';
	// Call to update the view
	for(;;)
	{
		int stopc = cvWaitKey(1);

		if (!isRunStep && stopc != 112 && stopc != 27 && stopc != 'v')
			stopc = 13;

		switch(stopc)
		{
			case 112:
				isRunStep = !isRunStep;
				break;
			case 13: //Enter to Load frame
				while (!vcap.read(frame))
				{
					cout << "No Load frame" << endl;
					//return -3;
					vcap.release();

					nFrame = 0;
					if (!vcap.open(fileVideo))
					{
						//No load file video
						return -1;
					}
				}

				std::cout << "Frame Id = " << nFrame++ << std::endl;
				
				frame.copyTo(frame_blur);
				frame.copyTo(mask);
				frame.copyTo(dst);

				if (isRunStep) break;
			case 'f': //Median Filter
				cv::medianBlur(frame, frame_blur, 2*b_size+1);
				cv::threshold(frame_blur, mask, th_size, 255, CV_THRESH_BINARY);
				mask.copyTo(dst);

				if (isRunStep) break;
			case 'b': //
				on_blobs();

				break;
			case 'm':
				on_morphology(2);
				
				if (isRunStep) break;
			case 't':
				cv::threshold(frame_blur, mask, th_size, 255, CV_THRESH_BINARY);
				mask.copyTo(dst);

				break;
			case '-':
				if (size)
				{
					size--;
					cvCreateTrackbar( "E & D Size: ", "Result window", &size, 20, on_size );
				}
				break;
			case '=':
				if (20 - size)
				{
					size++;
					cvCreateTrackbar( "E & D Size: ", "Result window", &size, 20, on_size );
				}
				break;
			case '[':
				on_trackbar_erosion(0);
				break;
			case ']':
				on_trackbar_dilation(0);
				break;
			case '\\':
				frame.copyTo(dst);
				break;
			case 'v':
				isViewZoom = !isViewZoom;
				if (!isViewZoom)
					destroyWindow("Result Zoom");
				break;
			case 27:
				return 0;
			default:;
		}
		
		ShowCombineImages("Result window", frame, frame_blur, mask, dst);
		if (isViewZoom)
			cv::imshow("Result Zoom",dst);
	}
	
	return 0;
}
#endif